#include <math.h>
#include "altestimator.h"

AltEstimator::AltEstimator(float dt, float kh, float kv)
	: dt_(dt)
	, kh_(kh)
	, kv_(kv)
	, h_(0)
	, v_(0)
	, maxd_(10)
{}

void AltEstimator::update(float h) {
	float h1 = h_ + v_ * dt_;
	float d = h - h1;

	if (fabsf(d) < maxd_) {
		h_ = h1 + d * kh_;
		v_ += d * kv_;
	} else
		h_ = h;
}
